#include "Atlas.h"
#include "Viewer.h"

using namespace std;

namespace msf
{

Atlas::Atlas()
{
    mpCurrentMap = static_cast<Map *>(NULL);
}

Atlas::Atlas(int initKFid) : mnLastInitKPFidMap(initKFid), mHasViewer(false)
{
    mpCurrentMap = static_cast<Map *>(NULL);
    CreateNewMap();
}

Atlas::~Atlas()
{
    for (std::set<Map *>::iterator it = mspMaps.begin(), end = mspMaps.end(); it != end;)
    {
        Map *pMi = *it;

        if (pMi)
        {
            delete pMi;
            pMi = static_cast<Map *>(NULL);

            it = mspMaps.erase(it);
        }
        else
            ++it;
    }
}

/**
 * @brief 创建新地图，如果当前活跃地图有效，先存储当前地图为不活跃地图，然后新建地图；否则，可以直接新建地图。
 * 
 */
void Atlas::CreateNewMap()
{
    //锁住地图集
    unique_lock<mutex> lock(mMutexAtlas);
    cout << "Atlas: Creation of new map with id: " << Map::nNextId << endl;
    //如果当前活跃地图有效，先存储当前地图为不活跃地图后退出
    if(mpCurrentMap)
    {
        // mnLastInitKPFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1
        if(!mspMaps.empty() && mnLastInitKPFidMap < mpCurrentMap->GetMaxKPFid())
            mnLastInitKPFidMap = mpCurrentMap->GetMaxKPFid() + 1; // 初始关键帧是当前地图最大关键帧的下一个index

        // 将当前地图保存起来
        mpCurrentMap->SetStoredMap();
        cout << "Atlas: Stored map with ID:" << mpCurrentMap->GetId() << endl;

    }
    cout << "Atlas: Creation of new map with last KPF id: " << mnLastInitKPFidMap << endl;

    mpCurrentMap = new Map(mnLastInitKPFidMap);  //新建地图
    mpCurrentMap->SetCurrentMap();              //设置为活跃地图
    mspMaps.insert(mpCurrentMap);               //插入地图集    
}

void Atlas::ChangeMap(Map *pMap)
{
    unique_lock<mutex> lock(mMutexAtlas);
    cout << "Change to map with id: " << pMap->GetId() << endl;
    if (mpCurrentMap)
    {
        mpCurrentMap->SetStoredMap();
    }

    mpCurrentMap = pMap;
    mpCurrentMap->SetCurrentMap();
}

unsigned long int Atlas::GetLastInitKPFid()
{
    unique_lock<mutex> lock(mMutexAtlas);
    return mnLastInitKPFidMap;
}

void Atlas::SetViewer(Viewer *pViewer)
{
    mpViewer = pViewer;
    mHasViewer = true;
}

void Atlas::AddKeyPoseFrame(KeyPoseFrame *pKPF)
{
    Map *pMapKPF = pKPF->GetMap();
    pMapKPF->AddKeyPoseFrame(pKPF);
}

void Atlas::InformNewBigChange()
{
    unique_lock<mutex> lock(mMutexAtlas);
    mpCurrentMap->InformNewBigChange();
}

int Atlas::GetLastBigChangeIdx()
{
    unique_lock<mutex> lock(mMutexAtlas);
    return mpCurrentMap->GetLastBigChangeIdx();
}

long unsigned Atlas::KeyPoseFramesInMap()
{
    unique_lock<mutex> lock(mMutexAtlas);
    return mpCurrentMap->KeyPoseFramesInMap();
}

std::vector<KeyPoseFrame *> Atlas::GetAllKeyPoseFrames()
{
    unique_lock<mutex> lock(mMutexAtlas);
    return mpCurrentMap->GetAllKeyPoseFrames();
}

vector<Map *> Atlas::GetAllMaps()
{
    unique_lock<mutex> lock(mMutexAtlas);
    struct compFunctor
    {
        inline bool operator()(Map *elem1, Map *elem2)
        {
            return elem1->GetId() < elem2->GetId();
        }
    };
    vector<Map *> vMaps(mspMaps.begin(), mspMaps.end());
    sort(vMaps.begin(), vMaps.end(), compFunctor());
    return vMaps;
}

int Atlas::CountMaps()
{
    unique_lock<mutex> lock(mMutexAtlas);
    return mspMaps.size();
}

void Atlas::clearMap()
{
    unique_lock<mutex> lock(mMutexAtlas);
    mpCurrentMap->clear();
}

void Atlas::clearAtlas()
{
    unique_lock<mutex> lock(mMutexAtlas);
    /*for(std::set<Map*>::iterator it=mspMaps.begin(), send=mspMaps.end(); it!=send; it++)
    {
        (*it)->clear();
        delete *it;
    }*/
    mspMaps.clear();
    mpCurrentMap = static_cast<Map *>(NULL);
    mnLastInitKPFidMap = 0;
}

Map *Atlas::GetCurrentMap()
{
    unique_lock<mutex> lock(mMutexAtlas);
    if (!mpCurrentMap)
        CreateNewMap();
    while (mpCurrentMap->IsBad())
        usleep(3000);

    return mpCurrentMap;
}

void Atlas::SetMapBad(Map *pMap)
{
    mspMaps.erase(pMap);
    pMap->SetBad();

    mspBadMaps.insert(pMap);
}

void Atlas::RemoveBadMaps()
{
    /*for(Map* pMap : mspBadMaps)
    {
        delete pMap;
        pMap = static_cast<Map*>(NULL);
    }*/
    mspBadMaps.clear();
}

bool Atlas::isInertial()
{
    unique_lock<mutex> lock(mMutexAtlas);
    return mpCurrentMap->IsInertial();
}

void Atlas::SetInertialSensor()
{
    unique_lock<mutex> lock(mMutexAtlas);
    // 接着又调用Map类的SetInertialSensor成员函数,将其设置为imu属性,以后的跟踪和预积分将和这个标志有关
    mpCurrentMap->SetInertialSensor();
}

void Atlas::SetImuInitialized()
{
    unique_lock<mutex> lock(mMutexAtlas);
    mpCurrentMap->SetImuInitialized();
}

bool Atlas::isImuInitialized()
{
    unique_lock<mutex> lock(mMutexAtlas);
    return mpCurrentMap->isImuInitialized();
}

/**
 * @brief 预保存，意思是在保存成地图文件之前，要保存到对应变量里面
 */
void Atlas::PreSave()
{

}

/**
 * @brief 后加载，意思是读取地图文件后加载各个信息
 */
void Atlas::PostLoad()
{

}

map<long unsigned int, KeyPoseFrame *> Atlas::GetAtlasKeyPoseframes()
{
    map<long unsigned int, KeyPoseFrame *> mpIdKPFs;
    for (Map *pMap_i : mvpBackupMaps)
    {
        vector<KeyPoseFrame *> vpKPFs_Mi = pMap_i->GetAllKeyPoseFrames();

        for (KeyPoseFrame *pKPF_j_Mi : vpKPFs_Mi)
        {
            mpIdKPFs[pKPF_j_Mi->mnId] = pKPF_j_Mi;
        }
    }

    return mpIdKPFs;
}


}